{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Downloaded 'Robot.pde'.\n",
      "Downloaded 'Hit.pde'.\n",
      "Downloaded 'World.pde'.\n"
     ]
    }
   ],
   "source": [
    "%download http://jupyter.cs.brynmawr.edu/hub/dblank/public/CS110%20Intro%20to%20Computing/2015/Lectures/Robot.pde\n",
    "%download http://jupyter.cs.brynmawr.edu/hub/dblank/public/CS110%20Intro%20to%20Computing/2015/Lectures/Hit.pde\n",
    "%download http://jupyter.cs.brynmawr.edu/hub/dblank/public/CS110%20Intro%20to%20Computing/2015/Lectures/World.pde"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "<div id=\"canvas_div_2\">\n",
       "  <b>Sketch #2:</b><br/>\n",
       "  <canvas id=\"canvas_2\"></canvas><br/>\n",
       "</div>\n",
       "<div id=\"controls_div_2\">\n",
       "  <button id=\"run_button_2\" onclick=\"startSketch('2');\">\n",
       "    <i class=\"fa fa-play-circle-o\" style=\"size: 2em;\"></i> \n",
       "        Run\n",
       "  </button>\n",
       "  <button id=\"pause_button_2\" onclick=\"pauseSketch('2');\">\n",
       "    <i class=\"fa fa-pause\" style=\"size: 2em;\"></i> \n",
       "        Pause\n",
       "  </button>\n",
       "  <button id=\"setup_button_2\" onclick=\"setupSketch('2');\">\n",
       "    setup()\n",
       "  </button>\n",
       "  <button id=\"draw_button_2\" onclick=\"drawSketch('2');\">\n",
       "    draw()\n",
       "  </button>\n",
       "</div>\n",
       "<b>Sketch #2 state:</b> <span id=\"state_2\">Loading...</span><br/>\n",
       "<script>\n",
       "\n",
       "function change_button(button, disable) {\n",
       "    button.disabled = disable;\n",
       "    if (disable) {\n",
       "        button.style.color = \"grey\";\n",
       "    } else {\n",
       "        button.style.color = \"black\";\n",
       "    }\n",
       "}\n",
       "      \n",
       "function startSketch(id) {\n",
       "    switchSketchState(id, true);\n",
       "}\n",
       "\n",
       "function pauseSketch(id) {\n",
       "    switchSketchState(id, false);\n",
       "}\n",
       "\n",
       "function drawSketch(id) {\n",
       "    var processingInstance = Processing.getInstanceById(\"canvas_\" + id);\n",
       "    if (processingInstance != undefined) {\n",
       "        if (processingInstance.draw != undefined) {\n",
       "            document.getElementById(\"state_\" + id).innerHTML = \"Drawing...\";\n",
       "            try {\n",
       "                processingInstance.redraw();  \n",
       "                document.getElementById(\"state_\" + id).innerHTML = \"Drawing... done! Paused.\";\n",
       "                document.getElementById(\"state_\" + id).style.color = \"blue\";\n",
       "            } catch (e) {\n",
       "                processingInstance.println(e.toString());\n",
       "                document.getElementById(\"state_\" + id).innerHTML = e.toString();\n",
       "                document.getElementById(\"state_\" + id).style.color = \"red\";\n",
       "            }\n",
       "        } else {\n",
       "            document.getElementById(\"state_\" + id).innerHTML = \"No drawing() function. Paused.\";\n",
       "            document.getElementById(\"state_\" + id).style.color = \"blue\";\n",
       "        }\n",
       "    } else {\n",
       "        document.getElementById(\"state_\" + id).innerHTML = \"Error.\";\n",
       "        document.getElementById(\"state_\" + id).style.color = \"red\";\n",
       "    }\n",
       "    change_button(document.getElementById(\"run_button_\" + id), processingInstance.draw == undefined);\n",
       "    change_button(document.getElementById(\"pause_button_\" + id), true);\n",
       "    change_button(document.getElementById(\"setup_button_\" + id), processingInstance.setup == undefined);\n",
       "    change_button(document.getElementById(\"draw_button_\" + id), processingInstance.draw == undefined);\n",
       "}\n",
       "\n",
       "function setupSketch(id) {\n",
       "    var processingInstance = Processing.getInstanceById(\"canvas_\" + id);\n",
       "    if (processingInstance != undefined) {\n",
       "        if (processingInstance.setup != undefined) {\n",
       "            document.getElementById(\"state_\" + id).innerHTML = \"Setting up...\";\n",
       "            try {\n",
       "                processingInstance.setup();  \n",
       "                document.getElementById(\"state_\" + id).innerHTML = \"Setting up... done! Paused.\";\n",
       "                document.getElementById(\"state_\" + id).style.color = \"blue\";\n",
       "            } catch (e) {\n",
       "                processingInstance.println(e.toString());\n",
       "                document.getElementById(\"state_\" + id).innerHTML = e.toString();\n",
       "                document.getElementById(\"state_\" + id).style.color = \"red\";\n",
       "            }\n",
       "        } else {\n",
       "            document.getElementById(\"state_\" + id).innerHTML = \"No setup() function. Paused.\";\n",
       "            document.getElementById(\"state_\" + id).style.color = \"blue\";\n",
       "        }\n",
       "    } else {\n",
       "        document.getElementById(\"state_\" + id).innerHTML = \"Error.\";\n",
       "        document.getElementById(\"state_\" + id).style.color = \"red\";\n",
       "    }\n",
       "    change_button(document.getElementById(\"run_button_\" + id), processingInstance.draw == undefined);\n",
       "    change_button(document.getElementById(\"pause_button_\" + id), true);\n",
       "    change_button(document.getElementById(\"setup_button_\" + id), processingInstance.setup == undefined);\n",
       "    change_button(document.getElementById(\"draw_button_\" + id), processingInstance.draw == undefined);\n",
       "}\n",
       "\n",
       "function switchSketchState(id, on) {\n",
       "    var processingInstance = Processing.getInstanceById(\"canvas_\" + id);\n",
       "    if (on) {\n",
       "        document.getElementById(\"state_\" + id).innerHTML = \"Running...\";\n",
       "        document.getElementById(\"state_\" + id).style.color = \"green\";\n",
       "        change_button(document.getElementById(\"run_button_\" + id), true);\n",
       "        change_button(document.getElementById(\"pause_button_\" + id), processingInstance.draw == undefined);\n",
       "        change_button(document.getElementById(\"setup_button_\" + id),  true);\n",
       "        change_button(document.getElementById(\"draw_button_\" + id), true);\n",
       "        processingInstance.loop();  // call Processing loop() function\n",
       "    } else {\n",
       "        document.getElementById(\"state_\" + id).innerHTML = \"Paused.\";\n",
       "        document.getElementById(\"state_\" + id).style.color = \"blue\";\n",
       "        change_button(document.getElementById(\"run_button_\" + id), processingInstance.draw == undefined);\n",
       "        change_button(document.getElementById(\"pause_button_\" + id), true);\n",
       "        change_button(document.getElementById(\"setup_button_\" + id), processingInstance.setup == undefined);\n",
       "        change_button(document.getElementById(\"draw_button_\" + id), processingInstance.draw == undefined);\n",
       "        processingInstance.noLoop(); // stop animation, call noLoop()\n",
       "    }\n",
       "}\n",
       "\n",
       "require([window.location.protocol + \"//calysto.github.io/javascripts/processing/processing.js\"], function () {\n",
       "    var processingCode = 'class World {\\n    float scale = 250;\\n    float at_x = 0;\\n    float at_y = 0;\\n    float w;\\n    float h;\\n    ArrayList<Robot> robots = new ArrayList<Robot>();\\n    ArrayList<ArrayList<PVector>> walls = new ArrayList<ArrayList<PVector>>();\\n    ArrayList<Integer> colors = new ArrayList<Integer>();\\n    World(int w, int h) {\\n        this.w = w; \\n        this.h = h;\\n        this.addWall(color(128, 0, 128), \\n                     new PVector(0.0, 0.0),\\n                     new PVector(this.w, 0.0),\\n                     new PVector(this.w, 10.0),\\n                     new PVector(0, 10.0));\\n        this.addWall(color(128, 0, 128), \\n                     new PVector(0, 0),\\n                     new PVector(0, this.h),\\n                     new PVector(10, this.h),\\n                     new PVector(10, 0));\\n        this.addWall(color(128, 0, 128), \\n                     new PVector(0.0, this.h - 10.0),\\n                     new PVector(0.0, this.h),\\n                     new PVector(this.w, this.h),\\n                     new PVector(this.w, this.h - 10.0)\\n                     );\\n        this.addWall(color(128, 0, 128), \\n                     new PVector(this.w - 10.0, 0.0),\\n                     new PVector(this.w, 0.0),\\n                     new PVector(this.w, this.h),\\n                     new PVector(this.w - 10.0, this.h));\\n    }\\n    void addBox(float x1, float y1, float x2, float y2, Integer col) {\\n      // add to world, scaled\\n      x1 = x1/this.w * this.w;\\n      y1 = y1/this.h * this.h;\\n      x2 = x2/this.w * this.w;\\n      y2 = y2/this.h * this.h;\\n      this.addWall(col, \\n                     new PVector(x1, y1),\\n                     new PVector(x2, y1),\\n                     new PVector(x2, y2),\\n                     new PVector(x1, y2));\\n    }\\n    void addWall(float x1, float y1, float x2, float y2) {\\n      // add to world, scaled\\n      this.addBox(x1, y1, x2, y2, color(128, 0, 128));\\n    }\\n    \\n    void setScale(float s) {\\n      // scale the world... > 1 make it bigger\\n      this.scale = s * 250;\\n    }\\n      \\n    void addWall(color c, PVector v1, PVector v2, PVector v3, PVector v4) {\\n        this.colors.add(c);\\n        ArrayList<PVector> wall = new ArrayList();\\n        wall.add(v1);\\n        wall.add(v2);\\n        wall.add(v3);\\n        wall.add(v4);\\n        this.walls.add(wall);\\n    }\\n    void addRobot(Robot robot) {\\n      // scale the robot\\'s position to this world:\\n      robot.x = robot.x/this.w * this.w;\\n      robot.y = robot.y/this.h * this.h;\\n      this.robots.add(robot);\\n      robot.world = this;\\n    }\\n    void update() {\\n        noStroke();\\n        fill(0, 128, 0);\\n        rect(this.at_x, this.at_y, this.w, this.h); \\n        int count = 0;\\n        for (ArrayList<PVector> wall: this.walls) {\\n            color c = (color)this.colors.get(count);\\n            fill(c);\\n            beginShape();\\n            for (PVector v: wall) {\\n                vertex(v.x, v.y);\\n            }\\n            endShape();\\n            count++;\\n        }\\n        for (Robot robot: this.robots) {\\n            robot.update();\\n            robot.draw();\\n        }\\n    }\\n}\\n\\n\\nclass Hit {\\n    float distance;\\n    float x;\\n    float y;\\n    float start_x;\\n    float start_y;\\n    color col;\\n    Hit(float x, float y, float distance, color col, \\n        float start_x, float start_y) {\\n      this.x = x;\\n      this.y = y;\\n      this.distance = distance;\\n      this.col = col;\\n      this.start_x = start_x;\\n      this.start_y = start_y;\\n    }\\n}\\n\\n\\n\\nclass Robot {\\n  float x;\\n  float y;\\n  float direction;\\n  boolean debug = false;\\n  float vx = 0.0; // velocity in x direction\\n  float vy = 0.0; // velocity in y direction\\n  float va = 0.0; // turn velocity\\n  World world;\\n  // sensors \\n  boolean stalled = false;\\n  String state = \"forward\";\\n  float time = 0;\\n  float[][] bounding_box = new float[4][2];\\n  color robot_color = color(255, 0, 0);\\n  Hit[] ir_sensors = new Hit[2];\\n  float max_ir = 50;\\n  Hit[] camera = new Hit[256];\\n  boolean cam = false;\\n\\n  Robot() {\\n    this.x = -1;\\n    this.y = -1;\\n    this.direction = 0;\\n    this.state = \"forward\";\\n  }\\n  Robot(float x, float y, float direction) {\\n    this.x = x;\\n    this.y = y;\\n    this.direction = direction;\\n    this.state = \"forward\";\\n  }\\n  void forward(float vx) {\\n    this.vx = vx;\\n  }\\n  void backward(float vx) {\\n    this.vx = -vx;\\n  }\\n  float getIR(int pos) {\\n    // 0 is on right, front\\n    // 1 is on left, front\\n    if (this.ir_sensors[pos] != null) {\\n      return this.ir_sensors[pos].distance/(this.max_ir/250.0 * this.world.scale);\\n    } else {\\n      return 1.0;\\n    }\\n  }\\n  PImage takePicture() {\\n    PImage pic = new PImage(256, 128);\\n    float size = max(this.world.w, this.world.h);\\n    for (int i=0; i<this.camera.length; i++) {\\n      Hit hit = this.camera[i];\\n      float high;\\n      Integer hcolor = null;\\n      if (hit != null) {\\n        float s = max(min(1.0 - hit.distance/size, 1.0), 0.0);\\n        float r = red(hit.col);\\n        float g = green(hit.col);\\n        float b = blue(hit.col);\\n        hcolor = color(r * s, g * s, b * s);\\n        high = (1.0 - s) * 128;\\n        //pg.line(i, 0 + high/2, i, 128 - high/2);\\n      } else {\\n        high = 0;\\n      }\\n      for (int j = 0; j < 128; j++) {\\n        if (j < high/2) { //256 - high/2.0) { // sky\\n          pic.set(i, j, color(0, 0, 128));\\n        } else if (j < 128 - high/2) { //256 - high && hcolor != null) { // hit\\n        if (hcolor != null)\\n          pic.set(i, j, hcolor);\\n        } else { // ground\\n          pic.set(i, j, color(0, 128, 0));\\n        }\\n      }\\n    }\\n    return pic;\\n  }\\n  void turn(float va) {\\n    this.va = va;\\n  }\\n  void stop() {\\n    this.vx = 0.0;\\n    this.vy = 0.0;\\n    this.va = 0.0;\\n  }\\n  boolean ccw(float ax, float ay, \\n  float bx, float by, \\n  float cx, float cy) {\\n    // counter clockwise\\n    return (((cy - ay) * (bx - ax)) > ((by - ay) * (cx - ax)));\\n  }\\n  boolean intersect(float ax, float ay, \\n  float bx, float by, \\n  float cx, float cy, \\n  float dx, float dy) {\\n    // Return true if line segments AB and CD intersect\\n    return (this.ccw(ax, ay, cx, cy, dx, dy) != this.ccw(bx, by, cx, cy, dx, dy) && \\n      this.ccw(ax, ay, bx, by, cx, cy) != this.ccw(ax, ay, bx, by, dx, dy));\\n  }\\n\\n  float[] coefs(float p1x, float p1y, float p2x, float p2y) {\\n    float A = (p1y - p2y);\\n    float B = (p2x - p1x);\\n    float C = (p1x * p2y - p2x * p1y);\\n    return new float[] {\\n      A, B, -C\\n    };\\n  }\\n\\n  float[] intersect_coefs(float L1_0, float L1_1, float L1_2, \\n  float L2_0, float L2_1, float L2_2) {\\n    float D  = L1_0 * L2_1 - L1_1 * L2_0;\\n    float Dx = L1_2 * L2_1 - L1_1 * L2_2;\\n    float Dy = L1_0 * L2_2 - L1_2 * L2_0;\\n    if (D != 0) {\\n      float x1 = Dx / D;\\n      float y1 = Dy / D;\\n      return new float[] {\\n        x1, y1\\n      };\\n    } else {\\n      return null;\\n    }\\n  }\\n  float[] intersect_hit(float p1x, float p1y, float p2x, float p2y, \\n  float p3x, float p3y, float p4x, float p4y ) {\\n    // http://stackoverflow.com/questions/20677795/find-the-point-of-intersecting-lines\\n    float[] L1 = coefs(p1x, p1y, p2x, p2y);\\n    float[] L2 = coefs(p3x, p3y, p4x, p4y);\\n    float[] xy = intersect_coefs(L1[0], L1[1], L1[2], \\n    L2[0], L2[1], L2[2]);\\n    // now check to see on both segments:\\n    if (xy != null) {\\n      float lowx = min(p1x, p2x) - .1;\\n      float highx = max(p1x, p2x) + .1;\\n      float lowy = min(p1y, p2y) - .1;\\n      float highy = max(p1y, p2y) + .1;\\n      if (lowx <= xy[0] && xy[0] <= highx && \\n        lowy <= xy[1] && xy[1] <= highy) {\\n        lowx = min(p3x, p4x) - .1;\\n        highx = max(p3x, p4x) + .1;\\n        lowy = min(p3y, p4y) - .1;\\n        highy = max(p3y, p4y) + .1;\\n        if (lowx <= xy[0] && xy[0] <= highx && \\n          lowy <= xy[1] && xy[1] <= highy) {\\n          return xy;\\n        }\\n      }\\n    }\\n    return null;\\n  }\\n  float distance(float x1, float y1, float x2, float y2) {\\n    return sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));\\n  } \\n  Hit castRay(float x1, float y1, float a, float maxRange) {\\n    ArrayList<Hit> hits = new ArrayList<Hit>();\\n    float x2 = sin(a) * maxRange + x1;\\n    float y2 = cos(a) * maxRange + y1;\\n    int count = 0;\\n    for (ArrayList<PVector> wall : this.world.walls) {\\n      // if intersection, can\\'t move\\n      PVector v1 = wall.get(0);\\n      PVector v2 = wall.get(1);\\n      PVector v3 = wall.get(2);\\n      PVector v4 = wall.get(3);\\n      float[] pos = this.intersect_hit(x1, y1, x2, y2, \\n      v1.x, v1.y, v2.x, v2.y);\\n      if (pos != null) {\\n        color col = this.world.colors.get(count);\\n        float dist = this.distance(pos[0], pos[1], x1, y1);\\n        hits.add(new Hit(pos[0], pos[1], dist, col, x1, y1));\\n      }\\n      pos = this.intersect_hit(x1, y1, x2, y2, \\n      v2.x, v2.y, v3.x, v3.y);\\n      if (pos != null) {\\n        color col = this.world.colors.get(count);\\n        float dist = this.distance(pos[0], pos[1], x1, y1);\\n        hits.add(new Hit(pos[0], pos[1], dist, col, x1, y1));\\n      }\\n      pos = this.intersect_hit(x1, y1, x2, y2, \\n      v3.x, v3.y, v4.x, v4.y);\\n      if (pos != null) {\\n        color col = this.world.colors.get(count);\\n        float dist = this.distance(pos[0], pos[1], x1, y1);\\n        hits.add(new Hit(pos[0], pos[1], dist, col, x1, y1));\\n      }\\n      pos = this.intersect_hit(x1, y1, x2, y2, \\n      v4.x, v4.y, v1.x, v1.y);\\n      if (pos != null) {\\n        color col = this.world.colors.get(count);\\n        float dist = this.distance(pos[0], pos[1], x1, y1);\\n        hits.add(new Hit(pos[0], pos[1], dist, col, x1, y1));\\n      }\\n      count++;\\n    }\\n    if (hits.size() == 0) {\\n      return null;\\n    } else {\\n      return min_hit(hits);\\n    }\\n  }\\n  Hit min_hit(ArrayList<Hit> hits) {\\n    Hit minimum = hits.get(0);\\n    for (Hit hit : hits) {\\n      if (hit.distance < minimum.distance) {\\n        minimum = hit;\\n      }\\n    }\\n    return minimum;\\n  }\\n  void brain() {\\n    // will be overridden\\n  }\\n  void update() {\\n    this.brain();\\n    float scale = this.world.scale;\\n    //this.direction += PI/180;\\n    float tvx = this.vx * sin(-this.direction + PI/2) + this.vy * cos(-this.direction + PI/2);\\n    float tvy = this.vx * cos(-this.direction + PI/2) - this.vy * sin(-this.direction + PI/2);\\n    // proposed positions:\\n    float px = this.x + tvx/250.0 * scale; \\n    float py = this.y + tvy/250.0 * scale; \\n    float pdirection = this.direction - this.va; \\n    // check to see if collision\\n    // bounding box:\\n    float[] p1 = rotateAround(px, py, 30/250.0 * scale, pdirection + PI/4 + 0 * PI/2);\\n    float[] p2 = rotateAround(px, py, 30/250.0 * scale, pdirection + PI/4 + 1 * PI/2);\\n    float[] p3 = rotateAround(px, py, 30/250.0 * scale, pdirection + PI/4 + 2 * PI/2);\\n    float[] p4 = rotateAround(px, py, 30/250.0 * scale, pdirection + PI/4 + 3 * PI/2);\\n    this.bounding_box[0] = p1;\\n    this.bounding_box[1] = p2;\\n    this.bounding_box[2] = p3;\\n    this.bounding_box[3] = p4;\\n    this.stalled = false;\\n    for (ArrayList<PVector> wall : this.world.walls) {\\n      // if intersection, can\\'t move\\n      PVector v1 = wall.get(0);\\n      PVector v2 = wall.get(1);\\n      PVector v3 = wall.get(2);\\n      PVector v4 = wall.get(3);\\n      if ( // p1 to p2\\n      this.intersect(p1[0], p1[1], p2[0], p2[1], \\n      v1.x, v1.y, v2.x, v2.y) ||\\n        this.intersect(p1[0], p1[1], p2[0], p2[1], \\n      v2.x, v2.y, v3.x, v3.y) ||\\n        this.intersect(p1[0], p1[1], p2[0], p2[1], \\n      v3.x, v3.y, v4.x, v4.y) ||\\n        this.intersect(p1[0], p1[1], p2[0], p2[1], \\n      v4.x, v4.y, v1.x, v1.y) ||\\n        // p2 to p3\\n      this.intersect(p2[0], p2[1], p3[0], p3[1], \\n      v1.x, v1.y, v2.x, v2.y) ||\\n        this.intersect(p2[0], p2[1], p3[0], p3[1], \\n      v2.x, v2.y, v3.x, v3.y) ||\\n        this.intersect(p2[0], p2[1], p3[0], p3[1], \\n      v3.x, v3.y, v4.x, v4.y) ||\\n        this.intersect(p2[0], p2[1], p3[0], p3[1], \\n      v4.x, v4.y, v1.x, v1.y) ||\\n        // p3 to p4\\n      this.intersect(p3[0], p3[1], p4[0], p4[1], \\n      v1.x, v1.y, v2.x, v2.y) ||\\n        this.intersect(p3[0], p3[1], p4[0], p4[1], \\n      v2.x, v2.y, v3.x, v3.y) ||\\n        this.intersect(p3[0], p3[1], p4[0], p4[1], \\n      v3.x, v3.y, v4.x, v4.y) ||\\n        this.intersect(p3[0], p3[1], p4[0], p4[1], \\n      v4.x, v4.y, v1.x, v1.y) ||\\n        // p4 to p1\\n      this.intersect(p4[0], p4[1], p1[0], p1[1], \\n      v1.x, v1.y, v2.x, v2.y) ||\\n        this.intersect(p4[0], p4[1], p1[0], p1[1], \\n      v2.x, v2.y, v3.x, v3.y) ||\\n        this.intersect(p4[0], p4[1], p1[0], p1[1], \\n      v3.x, v3.y, v4.x, v4.y) ||\\n        this.intersect(p4[0], p4[1], p1[0], p1[1], \\n      v4.x, v4.y, v1.x, v1.y)) {\\n        this.stalled = true;\\n        break;\\n        //this.x = this.x - tvx/250.0 * scale; \\n        //this.y = this.y - tvy/250.0 * scale;\\n      }\\n    }\\n    if (! this.stalled) {\\n      // if no intersection, make move\\n      this.x = px; \\n      this.y = py; \\n      this.direction = pdirection; \\n      if (tvx != 0 && random(1.0) < .01) {\\n        //this.direction += random(.1) - .05; // a bit of noise\\n      }\\n    } else {\\n      //this.direction += random(.2) - .1;\\n    }\\n    // update sensors, camera\\n    // on right:\\n    float[] p = rotateAround(this.x, this.y, 25/250.0 * scale, this.direction + PI/8);\\n    Hit hit = this.castRay(p[0], p[1], -this.direction + PI/2.0, this.max_ir/250.0 * scale);\\n    if (hit != null) {\\n      if (this.debug) {\\n        fill(0, 255, 0);\\n        ellipse(p[0], p[1], 5, 5);\\n        ellipse(hit.x, hit.y, 5, 5);\\n      }\\n      this.ir_sensors[0] = hit;\\n    } else {\\n      this.ir_sensors[0] = null;\\n    }\\n    p = rotateAround(this.x, this.y, 25/250.0 * scale, this.direction - PI/8);\\n    hit = this.castRay(p[0], p[1], -this.direction + PI/2, this.max_ir/250.0 * scale);\\n    if (hit != null) {\\n      if (this.debug) {\\n        fill(0, 0, 255);\\n        ellipse(p[0], p[1], 5, 5);\\n        ellipse(hit.x, hit.y, 5, 5);\\n      }\\n      this.ir_sensors[1] = hit;\\n    } else {\\n      this.ir_sensors[1] = null;\\n    }\\n    // camera:\\n    for (int i=0; i<256; i++) {\\n      float angle = i/256.0 * 60 - 30;  \\n      this.camera[i] = this.castRay(this.x, this.y, -this.direction + PI/2.0 - angle*PI/180.0, 1000);\\n    } \\n  }\\n  float[] rotateAround(float x1, float y1, float length, float angle) {\\n    return new float[] {\\n      x1 + length * cos(-angle), \\n      y1 - length * sin(-angle)\\n    };\\n  }\\n  void draw() {\\n    float scale = this.world.scale;\\n    float [] sx = new float[] {\\n      0.05, 0.05, 0.07, 0.07, 0.09, 0.09, 0.07, \\n      0.07, 0.05, 0.05, -0.05, -0.05, -0.07, \\n      -0.08, -0.09, -0.09, -0.08, -0.07, -0.05, \\n      -0.05\\n    };\\n    float [] sy = new float[] {\\n      0.06, 0.08, 0.07, 0.06, 0.06, -0.06, -0.06, \\n      -0.07, -0.08, -0.06, -0.06, -0.08, -0.07, \\n      -0.06, -0.05, 0.05, 0.06, 0.07, 0.08, 0.06\\n    };\\n    if (this.debug) {\\n      stroke(255);\\n      // bounding box:\\n      float[] p1 = rotateAround(this.x, this.y, 30/250.0 * scale, this.direction + PI/4.0 + 0 * PI/2.0);\\n      float[] p2 = rotateAround(this.x, this.y, 30/250.0 * scale, this.direction + PI/4.0 + 1 * PI/2.0);\\n      float[] p3 = rotateAround(this.x, this.y, 30/250.0 * scale, this.direction + PI/4.0 + 2 * PI/2.0);\\n      float[] p4 = rotateAround(this.x, this.y, 30/250.0 * scale, this.direction + PI/4.0 + 3 * PI/2.0);\\n      line(p1[0], p1[1], p2[0], p2[1]);\\n      line(p2[0], p2[1], p3[0], p3[1]);\\n      line(p3[0], p3[1], p4[0], p4[1]);\\n      line(p4[0], p4[1], p1[0], p1[1]);\\n    }\\n    pushMatrix();\\n    translate(this.x, this.y);\\n    rotate(this.direction);\\n    // body:\\n    if (this.stalled) {\\n      fill(128, 128, 128);\\n      stroke(255);\\n    } else {\\n      fill(this.robot_color);\\n      noStroke();\\n    }\\n    beginShape();\\n    for (int i =0; i < sx.length; i++) {\\n      vertex(sx[i] * scale, sy[i] * scale);\\n    }\\n    endShape();\\n    noStroke();\\n    // Draw wheels:\\n    fill(0);\\n    rect(-10/250.0 * scale, -23/250.0 * scale, 19/250.0 * scale, 5/250.0 * scale);\\n    rect(-10/250.0 * scale, 18/250.0 * scale, 19/250.0 * scale, 5/250.0 * scale);\\n    // hole:\\n    fill(0, 64, 0);\\n    ellipse(0, 0, 7/250.0 * scale, 7/250.0 * scale);\\n    // fluke\\n    fill(0, 64, 0);\\n    rect(15/250.0 * scale, -10/250.0 * scale, 4/250.0 * scale, 19/250.0 * scale);\\n    popMatrix();\\n    // draw sensors\\n    // right front IR\\n    // position of start of sensor:\\n    float[] p1 = rotateAround(this.x, this.y, 25/250.0 * scale, this.direction + PI/8);\\n    // angle of sensor:\\n    float[] p2 = rotateAround(p1[0], p1[1], this.getIR(0) * this.max_ir/250.0 * scale, this.direction);\\n    float dist = this.distance(p1[0], p1[1], p2[0], p2[1]);\\n    if (this.getIR(0) < 1.0) {\\n      stroke(255);\\n    }\\n    fill(128, 0, 128, 64);\\n    arc(p1[0], p1[1], dist * 2, dist * 2, \\n    this.direction - .5, this.direction + .5);\\n    // left front IR\\n    p1 = rotateAround(this.x, this.y, 25/250.0 * scale, this.direction - PI/8);\\n    p2 = rotateAround(p1[0], p1[1], this.getIR(1) * this.max_ir/250.0 * scale, this.direction);\\n    dist = this.distance(p1[0], p1[1], p2[0], p2[1]);\\n    if (this.getIR(1) < 1.0) {\\n      stroke(255);\\n    }\\n    fill(128, 0, 128, 64);\\n    arc(p1[0], p1[1], dist * 2, dist * 2, \\n    this.direction - .5, this.direction + .5);\\n  }\\n}\\n\\n\\n\\n\\nRobot robot;\\nWorld world;\\nint mx, my;\\n\\nvoid mousePressed() {\\n    mx = mouseX;\\n    my = mouseY;\\n}\\n\\nvoid mouseDragged() {\\n    robot.forward((mouseX - mx)/100.0);\\n    robot.turn(-(mouseY - my)/1000.0);\\n}\\n\\nvoid mouseReleased() {\\n    //robot.stop();\\n    robot.state = \"stop\";\\n}\\n\\nclass MyRobot extends Robot {\\n    MyRobot(float x, float y, float r) {\\n        super(x, y, r);\\n        this.state = \"start\";\\n    }\\n\\n    void brain() {\\n        PImage pic = this.takePicture();\\n        // Show pic on canvas:\\n        image(pic, 500 - 256, 250);\\n        fill(0);\\n        text(\"getIR(0): \" + nf(this.getIR(0), 1, 4), 0, 265);\\n        text(\"getIR(1): \" + nf(this.getIR(1), 1, 4), 0, 280);\\n        text(\"stalled: \" + this.stalled, 0, 295);\\n        text(\"state: \" + this.state, 0, 310);\\n        text(\"time: \" + this.time, 0, 325);\\n        float speed = 3.0;\\n        // Start of good stuff:\\n        if (this.state == \"start\") {\\n            this.forward(3);\\n            this.state = \"s1\";\\n        } else if (this.state == \"s1\") {\\n            if (this.stalled) {\\n                this.state = \"s2\";\\n            } else {\\n                this.forward(3);\\n            }\\n        } else if (this.state == \"s2\") {\\n            this.backward(3);\\n            this.state = \"s3\";\\n        } else if (this.state == \"s3\") {\\n            if (this.stalled) {\\n                this.forward(3);\\n                this.state = \"s4\";\\n            } else {\\n                this.backward(3);\\n            }\\n        } else if (this.state == \"s4\") {\\n            if (this.getIR(0) < 1) {\\n                this.state = \"s5\";\\n            } else {\\n                this.forward(3);\\n            }\\n        } else if (this.state == \"s5\") {\\n            this.stop();\\n        }\\n    }\\n}\\n\\nvoid setup() {\\n    // Set up canvas 500, 250 for World, 128 for camera:\\n    size(500, 250 + 128);\\n    // Set rate so it doesn\\'t overwhelm your browser:\\n    frameRate(10);\\n    // Create a simulated world:\\n    world = new World(500, 250);\\n    world.addWall(100, 0, 110, 110);  \\n    world.addBox(200, 95, 210, 170, color(255, 0, 255));  \\n    world.addBox(250, 95, 260, 130, color(255, 255, 0));  \\n    world.addBox(300, 190, 310, 240, color(255, 128, 0));  \\n    // Create robot, and add to world:\\n    robot = new MyRobot(400, 100, 0);\\n    world.addRobot(robot);\\n}\\n\\nvoid draw() {\\n    // Clear:\\n    background(255);\\n    // Run simulation for one step:\\n    world.update();\\n}';\n",
       "    var cc;\n",
       "    var processingInstance;\n",
       "    var has_error = false;\n",
       "    try {\n",
       "        cc = Processing.compile(processingCode);\n",
       "    } catch (e) {\n",
       "        console.log(e);\n",
       "        cc = Processing.compile(\"println('\" + e.toString() + \"');\");\n",
       "        document.getElementById(\"state_2\").innerHTML = e.toString();\n",
       "        document.getElementById(\"state_2\").style.color = \"red\";\n",
       "        has_error = true;\n",
       "    }\n",
       "    if (cc != undefined) {\n",
       "        try {\n",
       "            processingInstance = new Processing(\"canvas_2\", cc);\n",
       "        } catch (e) {\n",
       "            console.log(e);\n",
       "            cc = Processing.compile(\"println('\" + e.toString() + \"');\");\n",
       "            document.getElementById(\"state_2\").innerHTML = e.toString();\n",
       "            document.getElementById(\"state_2\").style.color = \"red\";\n",
       "            processingInstance = new Processing(\"canvas_2\", cc);\n",
       "            has_error = true;\n",
       "        }\n",
       "    }\n",
       "    if (processingInstance != undefined) {\n",
       "        setTimeout(function () {\n",
       "            // Canvas:\n",
       "            if (processingInstance.externals.context === undefined) {\n",
       "                document.getElementById(\"canvas_div_2\").style.display = \"none\";\n",
       "            } \n",
       "        }, 100);\n",
       "        // Controls:\n",
       "        if (!(processingInstance.isRunning() && processingInstance.draw != undefined)) {\n",
       "            document.getElementById(\"controls_div_2\").style.display = \"none\";\n",
       "        } \n",
       "        if (processingInstance.draw != undefined) {\n",
       "            if (!has_error) {\n",
       "                document.getElementById(\"state_2\").innerHTML = \"Running...\";\n",
       "                document.getElementById(\"state_2\").style.color = \"green\";\n",
       "            }\n",
       "            change_button(document.getElementById(\"run_button_2\"), true);\n",
       "            change_button(document.getElementById(\"pause_button_2\"), false);\n",
       "            change_button(document.getElementById(\"setup_button_2\"),  true);\n",
       "            change_button(document.getElementById(\"draw_button_2\"), true);\n",
       "        } else {\n",
       "            if (!has_error) {\n",
       "                document.getElementById(\"state_2\").innerHTML = \"Done.\";\n",
       "                document.getElementById(\"state_2\").style.color = \"blue\";\n",
       "            }\n",
       "            change_button(document.getElementById(\"run_button_2\"), true);\n",
       "            change_button(document.getElementById(\"pause_button_2\"), true);\n",
       "            change_button(document.getElementById(\"setup_button_2\"),  processingInstance.setup == undefined);\n",
       "            change_button(document.getElementById(\"draw_button_2\"), true);\n",
       "        }\n",
       "    } else {\n",
       "        document.getElementById(\"canvas_div_2\").style.display = \"none\";\n",
       "        document.getElementById(\"controls_div_2\").style.display = \"none\";\n",
       "        if (!has_error) {\n",
       "            document.getElementById(\"state_2\").innerHTML = \"Error.\";\n",
       "            document.getElementById(\"state_2\").style.color = \"red\";\n",
       "        }\n",
       "        change_button(document.getElementById(\"run_button_2\"), true);\n",
       "        change_button(document.getElementById(\"pause_button_2\"), true);\n",
       "        change_button(document.getElementById(\"setup_button_2\"),  true);\n",
       "        change_button(document.getElementById(\"draw_button_2\"), true);\n",
       "    }\n",
       "});\n",
       "\n",
       "</script>\n"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "%include Robot.pde\n",
    "%include Hit.pde\n",
    "%include World.pde\n",
    "\n",
    "Robot robot;\n",
    "World world;\n",
    "int mx, my;\n",
    "\n",
    "void mousePressed() {\n",
    "    mx = mouseX;\n",
    "    my = mouseY;\n",
    "}\n",
    "\n",
    "void mouseDragged() {\n",
    "    robot.forward((mouseX - mx)/100.0);\n",
    "    robot.turn(-(mouseY - my)/1000.0);\n",
    "}\n",
    "\n",
    "void mouseReleased() {\n",
    "    //robot.stop();\n",
    "    robot.state = \"stop\";\n",
    "}\n",
    "\n",
    "class MyRobot extends Robot {\n",
    "    MyRobot(float x, float y, float r) {\n",
    "        super(x, y, r);\n",
    "        this.state = \"start\";\n",
    "    }\n",
    "\n",
    "    void brain() {\n",
    "        PImage pic = this.takePicture();\n",
    "        // Show pic on canvas:\n",
    "        image(pic, 500 - 256, 250);\n",
    "        fill(0);\n",
    "        text(\"getIR(0): \" + nf(this.getIR(0), 1, 4), 0, 265);\n",
    "        text(\"getIR(1): \" + nf(this.getIR(1), 1, 4), 0, 280);\n",
    "        text(\"stalled: \" + this.stalled, 0, 295);\n",
    "        text(\"state: \" + this.state, 0, 310);\n",
    "        text(\"time: \" + this.time, 0, 325);\n",
    "        float speed = 3.0;\n",
    "        // Start of good stuff:\n",
    "        if (this.state == \"start\") {\n",
    "            this.forward(3);\n",
    "            this.state = \"s1\";\n",
    "        } else if (this.state == \"s1\") {\n",
    "            if (this.stalled) {\n",
    "                this.state = \"s2\";\n",
    "            } else {\n",
    "                this.forward(3);\n",
    "            }\n",
    "        } else if (this.state == \"s2\") {\n",
    "            this.backward(3);\n",
    "            this.state = \"s3\";\n",
    "        } else if (this.state == \"s3\") {\n",
    "            if (this.stalled) {\n",
    "                this.forward(3);\n",
    "                this.state = \"s4\";\n",
    "            } else {\n",
    "                this.backward(3);\n",
    "            }\n",
    "        } else if (this.state == \"s4\") {\n",
    "            if (this.getIR(0) < 1) {\n",
    "                this.state = \"s5\";\n",
    "            } else {\n",
    "                this.forward(3);\n",
    "            }\n",
    "        } else if (this.state == \"s5\") {\n",
    "            this.stop();\n",
    "        }\n",
    "    }\n",
    "}\n",
    "\n",
    "void setup() {\n",
    "    // Set up canvas 500, 250 for World, 128 for camera:\n",
    "    size(500, 250 + 128);\n",
    "    // Set rate so it doesn't overwhelm your browser:\n",
    "    frameRate(10);\n",
    "    // Create a simulated world:\n",
    "    world = new World(500, 250);\n",
    "    world.addWall(100, 0, 110, 110);  \n",
    "    world.addBox(200, 95, 210, 170, color(255, 0, 255));  \n",
    "    world.addBox(250, 95, 260, 130, color(255, 255, 0));  \n",
    "    world.addBox(300, 190, 310, 240, color(255, 128, 0));  \n",
    "    // Create robot, and add to world:\n",
    "    robot = new MyRobot(400, 100, 0);\n",
    "    world.addRobot(robot);\n",
    "}\n",
    "\n",
    "void draw() {\n",
    "    // Clear:\n",
    "    background(255);\n",
    "    // Run simulation for one step:\n",
    "    world.update();\n",
    "}\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Calysto Processing",
   "language": "processing",
   "name": "calysto_processing"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "text/x-java",
    "version": 2
   },
   "file_extension": ".java",
   "mimetype": "text/x-java",
   "name": "java"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}